#pragma config(Sensor, in1,    rightSensor,    sensorLineFollower)
#pragma config(Sensor, in2,    middleSensor,   sensorLineFollower)
#pragma config(Sensor, in3,    leftSensor,     sensorLineFollower)
#pragma config(Sensor, dgtl1,  leftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  rightEncoder,   sensorQuadEncoder)
#pragma config(Motor,  port1,           leftMotor,     tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port10,          rightMotor,    tmotorVex393, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
	wait1Msec(2000);
//robot waits 2 seconds before moving
	while(SensorValue[middleSensor]<= 2500 || SensorValue[rightSensor]<= 2500 || SensorValue[leftSensor] <= 2500)
		//goes foward and sops at first yeld sign at the intersection
		//values of the darkness or color of the lines
	{
		if(SensorValue[middleSensor]>= 2500)
			//if the middle sensor reads 2500, the motor will start moving at the certain speed
		{
			motor[rightMotor] = 60;
			motor[leftMotor] = 60;
		}
		else if(SensorValue[leftSensor]>= 2500)
			//if the left motor is reading 2500, then that means that the sensor/robot is not on the line and will move
		{
			motor[leftMotor] = 45;
			motor[rightMotor] = 60;
		}
		else if(SensorValue[rightSensor]>= 2500)
			//if the right motor is reading 2500, then that
//means that the sensor/robot is not on the line and will move
		{
			motor[leftMotor] = 60;
			motor[rightMotor] = 45;
		}
	}

	//motor[leftMotor] = 45;
	//motor[rightMotor] = 45;


	motor[leftMotor] = 30;
	motor[rightMotor] = 30;
	wait1Msec(150);
	//goes foward a bit so the sensors won't be on the right tape of the first intersection
	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(1000);



	//this allows the robot sensors to be on the tape when it moves



	while(SensorValue[rightSensor] <= 2500 || SensorValue[middleSensor] <= 2500|| SensorValue[leftSensor] <= 2500)
		//Robot continues until the big 4-way intersection where it will stop
{
		if(SensorValue[middleSensor] >= 2500)
		{
			motor[leftMotor] = 60;
			motor[rightMotor] = 60;
		}
		else if(SensorValue[rightSensor] >= 2500)
		{
			motor[leftMotor] = 60;
			motor[rightMotor] = 45;
		}
		else if(SensorValue[leftSensor] >= 2500)
		{
			motor[leftMotor] = 45;
			motor[rightMotor] = 60;
		}
}

	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(1000);



SensorValue[leftEncoder] = 370;
SensorValue[rightEncoder] = 370;
while(SensorValue[leftEncoder] < 370 || SensorValue[rightEncoder] < 370)
	//robot turns left 90 degrees
{
	motor[rightMotor] = 60;
	motor[leftMotor] = -60;

if(SensorValue[leftEncoder] >= 370)
{
	motor[leftMotor] = 0;
}

if(SensorValue[rightEncoder] >= 370)
{
	motor[rightMotor] = 0;
}
}

	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(1000);

	while(SensorValue[middleSensor]<= 2500 || SensorValue[leftSensor]<= 2500 || SensorValue[rightSensor] <= 2500)
		//robot continues along the line until the intersection of current tape and left turn
		//values of the darkness or color of the lines
	{
		if(SensorValue[middleSensor]>= 2500)
			//if the middle sensor reads 2500, the motor will start moving at the certain speed
		{
			motor[rightMotor] = 60;
			motor[leftMotor] = 60;
		}
		else if(SensorValue[leftSensor]>= 2500)
			//if the left motor is reading 2500, then that means that the sensor/robot is not on the line and will move
		{
			motor[leftMotor] = 45;
			motor[rightMotor] = 60;
		}
		else if(SensorValue[rightSensor]>= 2500)
			//if the right motor is reading 2500, then that means that the sensor/robot is not on the line and will move
		{
			motor[leftMotor] = 60;
			motor[rightMotor] = 45;
		}
	}

	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(1000);

	SensorValue[rightEncoder] = 0;
	SensorValue[leftEncoder] = 0;
	while(SensorValue[leftEncoder] < 370 || SensorValue[rightEncoder] < 370)
		//robot turns left 90 degrees
	{
		motor[rightMotor] = 60;
		motor[leftMotor] = -60;
		if(SensorValue[leftEncoder] >= 370)
			//if the left sensor is reading 2500 or lower, then the robot will turn left again
		{
			motor[leftMotor] = 0;
		}
		if(SensorValue[rightEncoder] >= 370)
		{
			motor[rightMotor] = 0;
		}
	}

	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(1000);

	while(SensorValue[middleSensor]<= 2500 || SensorValue[rightSensor]<= 2500 || SensorValue[leftSensor] <= 2500)
		//robot continues along the tape line until it reaches the intersection of current tape and right tape
		//values of the darkness or color of the lines
	{
		if(SensorValue[middleSensor]>= 2500)
			//if the middle sensor reads 2500, the motor will start moving at the certain speed
		{
			motor[rightMotor] = 60;
			motor[leftMotor] = 60;
		}
		else if(SensorValue[leftSensor]>= 2500)
			//if the left motor is reading 2500, then that means that the sensor/robot is not on the line and will move
		{
			motor[leftMotor] = 45;
			motor[rightMotor] = 60;
		}
		else if(SensorValue[rightSensor]>= 2500)
			//if the right motor is reading 2500, then that means that the sensor/robot is not on the line and will move
		{
			motor[leftMotor] = 60;
			motor[rightMotor] = 45;
		}
	}

	motor[leftSensor] = 0;
	motor[rightSensor] = 0;
	wait1Msec(1000);

	SensorValue[leftEncoder] = 0;
	SensorValue[rightEncoder] = 0;
	while(SensorValue[rightEncoder] > -370 || SensorValue[leftEncoder] > -370)
		//robot turns right

	{
		motor[leftMotor] = 60;
		motor[rightMotor] = -60;
		if(SensorValue[rightEncoder] <= -370)

		{

			motor[rightMotor] = 0;
		}
		if(SensorValue[leftEncoder] <= -370)
		{
			motor[leftMotor] = 0;
		}
	}


	////////////////////////////////////////////////////////////////////////

	SensorValue[leftEncoder] = 0;
	SensorValue[rightEncoder] = 0;
	while(SensorValue[rightEncoder] < -370 || SensorValue[leftEncoder] < -370)
		//robot turns right again to go back to the start point
	{
		motor[leftMotor] = 60;
		motor[rightMotor] = -60;

		if(SensorValue[rightEncoder] >= -370)
		{
			motor[rightMotor] = 0;
		}

		if(SensorValue[leftEncoder] >= -370)
		{
			motor[leftMotor] = 0;
		}
}

	motor[rightMotor] = 0;
	motor[leftMotor] = 0;
	wait1Msec(1000);


	while(SensorValue[middleSensor]<= 2500 || SensorValue[rightSensor]<= 2500 || SensorValue[leftSensor] <= 2500)
		//robot continues along the tape line until it reaches the intersection of current tape and right tape
		//values of the darkness or color of the lines
	{
		if(SensorValue[middleSensor]>= 2500)
			//if the middle sensor reads 2500, the motor will start moving at the certain speed
		{
			motor[rightMotor] = 60;
			motor[leftMotor] = 60;
		}
		else if(SensorValue[leftSensor]>= 2500)
			//if the left motor is reading 2500, then that means that the sensor/robot is not on the line and will move
		{
			motor[leftMotor] = 45;
			motor[rightMotor] = 60;
		}
		else if(SensorValue[rightSensor]>= 2500)
			//if the right motor is reading 2500, then that means that the sensor/robot is not on the line and will move
		{
			motor[leftMotor] = 60;
			motor[rightMotor] = 45;
		}
}

	motor[rightMotor] = 0;
	motor[leftMotor] = 0;
	wait1Msec(1000);

SensorValue[leftEncoder] = -370;
SensorValue[rightEncoder] = -370;
while(SensorValue[leftEncoder] > -370 || SensorValue[rightEncoder] > -370)
	//robot turns right 90 degrees
	{
	motor[rightMotor] = -60;
	motor[leftMotor] = 60;

if(SensorValue[leftEncoder] <= -370)
		{
	motor[leftMotor] = 0;
		}

if(SensorValue[rightEncoder] <= -370)
		{
	motor[rightMotor] = 0;
		}
}

	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(1000);

	while(SensorValue[middleSensor]<= 2500 || SensorValue[rightSensor]<= 2500 || SensorValue[leftSensor] <=2500)
		//robot continues along the tape line until it reaches the intersection of current tape and right tape
		//values of the darkness or color of the lines
	{
		if(SensorValue[middleSensor]>= 2500)
			//if the middle sensor reads 2500, the motor will start moving at the certain speed
		{
			motor[rightMotor] = 60;
			motor[leftMotor] = 60;
		}
		else if(SensorValue[leftSensor]>= 2500)
			//if the left motor is reading 2500, then that means that the sensor/robot is not on the line and will move
		{
			motor[leftMotor] = 45;
			motor[rightMotor] = 60;
		}
		else if(SensorValue[rightSensor]>= 2500)
			//if the right motor is reading 2500, then that means that the sensor/robot is not on the line and will move
		{
			motor[leftMotor] = 60;
			motor[rightMotor] = 45;
		}
	}



	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(1000);

SensorValue[leftEncoder] = -370;
SensorValue[rightEncoder] = -370;
while(SensorValue[leftEncoder] > -370 || SensorValue[rightEncoder] > -370)
	//robot turns right 90 degrees
	{
	motor[rightMotor] = -60;
	motor[leftMotor] = 60;

if(SensorValue[leftEncoder] <= -370)
		{
	motor[leftMotor] = 0;
		}

if(SensorValue[rightEncoder] <= -370)
		{
	motor[rightMotor] = 0;
		}
	}

	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(1000);

	while(SensorValue[middleSensor]<= 2500 || SensorValue[leftSensor]<= 2500 || SensorValue[rightSensor] <= 2500)
		//robot continues along the tape line until it reaches the intersection of current tape and right tape
		//values of the darkness or color of the lines
	{
		if(SensorValue[middleSensor]>= 2500)
			//if the middle sensor reads 2500, the motor will start moving at the certain speed
		{
			motor[rightMotor] = 60;
			motor[leftMotor] = 60;
		}
		else if(SensorValue[leftSensor]>= 2500)
			//if the left motor is reading 2500, then that means that the sensor/robot is not on the line and will move
		{
			motor[leftMotor] = 45;
			motor[rightMotor] = 60;
		}
		else if(SensorValue[rightSensor]>= 2500)
			//if the right motor is reading 2500, then that means that the sensor/robot is not on the line and will move
		{
			motor[leftMotor] = 60;
			motor[rightMotor] = 45;
		}
	}

	//motor[leftMotor] = 45;
	//motor[rightMotor] = 45;


	motor[leftMotor] = 30;
	motor[rightMotor] = 30;
	wait1Msec(150);
	//goes foward a bit so the sensors won't be on the right tape of the first intersection
	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(1000);


}



//////////////////////////////////////
